#!/usr/bin/python

import serial
import time
class Robbus:
	_ser = None

	def _sendWrapped(self, c, sum):
		if ord(c) < 4:
			self._ser.write("\x00")
			x = self._ser.read(1)
			print ">", x, ord(x)
			d = chr(ord(c)+4)
			self._ser.write(d)
			x = self._ser.read(1)
			print ">", x, ord(x)
		else:
			self._ser.write(c)
			x = self._ser.read(1)
			print ">", x, ord(x)
		return sum+ord(c)

	def _sendMessage(self, alias,data,mask = None):
		sum = 0;
		if mask is None:
			self._ser.write("\x02")     #write regular packet start
		else:
			self._ser.write("\x03")     #write group packet start
		x = self._ser.read(1)
		print ">", x, ord(x)
		sum = self._sendWrapped(alias,sum) 
		if mask is not None:
			sum = self._sendWrapped(mask,sum) 	
		sum = self._sendWrapped(chr(len(data)),sum)
		for c in data:
			sum = self._sendWrapped(c,sum)	
		outSum = (256-(sum % 256)) % 256
		self._sendWrapped(chr(outSum),sum)

	def _dewrapRead(self):
		x = self._ser.read(1)
		print "<", x, ord(x)
		if ord(x) == 0:
			x = self._ser.read(1)	
			print "<", x, ord(x)
			return chr(ord(x)-4)
		else:
			return x

	# read reply
	def _readReply(self):
		# consume outgoing message
		s = ""
		# and read incomming
		print "Receiving data..."
		x = self._ser.read(1) # head, no need to dewrap
		s += x
		print "<", x, ord(x)
		x = self._ser.read(1) # head, no need to dewrap
		s += x
		print "<", x, ord(x)
		lenstr = self._dewrapRead()
		paylen = ord(lenstr);
		s += lenstr
		while paylen > 0:
			s += self._dewrapRead();
			paylen = paylen-1
		s += self._dewrapRead() #chsum
		return s

	def _parseReply(self, s):
		if len(s) < 3:
			print "Input too short"
			return
		s = s [1:] # cut head
		chSum = sum([ord(c) for c in s]) 
		if sum([ord(c) for c in s]) % 256 != 0:
			print "Invalid checksum", (chSum%256), (256-(chSum%256))
			return -1
		address = chr(ord(s[0]) - 128)
		print "Reply from:", address, " hex:", address.encode("hex")
		replySize = ord(s[1])
		print "      size:", replySize
		if replySize != len (s[2:-1]):
			print "Data size doesn't match real data length"
		data = s[2:-1]
		print "      data:", [ord(i) for i in data], " hex:", data.encode("hex")
		return 0

	def open(self, port='/dev/robbus', baudrate=115200):
		self._ser = serial.Serial(port, baudrate, timeout=1)
	
	def close(self):
		self._ser.close()
		self._ser = None
	
	def send(self, address, data):
		print "Sending..."
		self._sendMessage(address,data)
		print "Receiving..."
		self._parseReply(self._readReply())
